#ifndef POISSON_RRT_
#define POISSON_RRT_

#include "ompl/geometric/planners/PlannerIncludes.h"
#include "ompl/datastructures/NearestNeighbors.h"
#include "IIterationCount.h"
#include <boost/thread/mutex.hpp>
#include <boost/thread/shared_mutex.hpp>
#include "PoissonState.h"

namespace poisson
{
    /** \brief Rapidly-exploring Random Trees */
    class PoissonRRT : public ompl::base::Planner, public IIterationCount
	{
    public:

        /** \brief Constructor */
        PoissonRRT(const ompl::base::SpaceInformationPtr &si);

        virtual ~PoissonRRT(void);

        virtual void getPlannerData(ompl::base::PlannerData &data) const;

		virtual ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &ptc);

        virtual void clear(void);

        void setGoalBias(double goalBias)
        {
            goalBias_ = goalBias;
        }

        double getGoalBias(void) const
        {
            return goalBias_;
        }

        void setRange(double distance)
        {
            maxDistance_ = distance;
        }

        double getRange(void) const
        {
            return maxDistance_;
        }

        /** \brief Set a different nearest neighbors datastructure */
        template<template<typename T> class NN>
        void setNearestNeighbors(void)
        {
            nn_.reset(new NN<Motion*>());
        }

        virtual void setup(void);

    protected:

        /** \brief Representation of a motion

            This only contains pointers to parent motions as we
            only need to go backwards in the tree. */
        class Motion
        {
        public:

            Motion(void) : state(NULL), parent(NULL)
            {
            }

            ~Motion(void)
            {
            }

            /** \brief The state contained by the motion */
			PoissonState		*state;

            /** \brief The parent motion in the exploration tree */
            Motion            *parent;

        };

        /** \brief Free the memory allocated by this planner */
        void freeMemory(void);

        /** \brief Compute distance between motions (actually distance between contained states) */
        double distanceFunction(const Motion* a, const Motion* b) const
        {
            return si_->distance(a->state->getState(), b->state->getState());
        }

        /** \brief State sampler */
        ompl::base::ValidStateSamplerPtr                          sampler_;

        /** \brief A nearest-neighbors datastructure containing the tree of motions */
        boost::shared_ptr< ompl::NearestNeighbors<Motion*> > nn_;


        /** \brief The fraction of time the goal is picked as the state to expand towards (if such a state is available) */
        double                                         goalBias_;

        /** \brief The maximum length of a motion to be added to a tree */
        double                                         maxDistance_;

        /** \brief The most recent goal motion.  Used for PlannerData computation */
        Motion                                         *lastGoalMotion_;

		struct SolutionInfo
		{
			Motion      *solution;
			Motion      *approxsol;
			double       approxdif;
			boost::mutex lock;
			std::vector<unsigned int> iterations;
		};
    };


}

#endif
